#include "Plane.h"
namespace math_tools
{
	Plane::Plane()
	{
		position_ = Vector3D(0.0, 0.0, 0.0);
		normal_ = Vector3D(1.0, 0.0, 0.0);
	}

	Plane::Plane(const Vector3D & position, const Vector3D & normal)
	{
		position_ = position;
		normal_ = normal;
		// Must be unit!
		normal_.unit();
	}

	Plane::~Plane()
	{

	}

	// Simple projection on the plane
	Vector3D Plane::closestPoint(const Vector3D & point)
	{
		return point-normal_*distanceFrom(point);
	}

	double Plane::distanceFrom(const Vector3D & point)
	{
		return sqrt(distanceFromSqr(point));
	}

	double Plane::distanceFromSqr(const Vector3D & point)
	{
		double distance = (point-position_).dot(normal_);
		Vector3D temp = point - normal_*distance;
		return (temp-point).magSqr();
	}
}
